#include "pid.h"

pid_ctrl::pid_ctrl(float kp, float ki, float kd, float max_pwr,
                   float blind_area, float max_I_put, float II_start,
                   float aim) {
  if (ki != 0)
    this->max_II = max_I_put / ki;
  else
    this->max_II = 0;
  this->kp = kp;
  this->ki = ki;
  this->kd = kd;
  this->max_pwr = max_pwr;
  this->max_I_put = max_I_put;
  this->aim = aim;
  this->blind_area = blind_area;

  // 开始积分的范围, fabs(err) < II_start--> II
  if (II_start == 0)
    this->II_start = this->blind_area * 10;
  else
    this->II_start = II_start;

  this->err_last = 74000;  // 第一次的初始化值,防止第一次的DD计算错误
  this->D_tolerance = blind_area / 10;  // 认为几乎静止的速度变化量
}

float pid_ctrl::pid_calc() {
  // 首次初始化
  if (begin_dir == 74) begin_dir = sgn(err_now);
  if (err_last == 74000) err_last = err_now;

  // 比例P
  if (fabs(err_now) <= blind_area) err_now = 0;  // 盲处理区
  P_put = kp * err_now;

  // 积分I
  if (fabs(err_now) > II_start) {
    II = 0;
  } else {
    II += err_now;
    if (fabs(II) > max_II) II = sgn(II) * max_II;
  }
  I_put = ki * II;
  if (fabs(I_put) > max_I_put) I_put = sgn(I_put) * max_I_put;

  // 微分D
  DD = err_now - err_last;  // 计算误差的微分量
  err_last = err_now;       // 更新前一次误差
  D_put = kd * DD;

  // 相加输出
  puts = P_put + I_put + D_put;
  if (fabs(puts) > max_pwr) puts = sgn(puts) * max_pwr;

  // 退出条件：如果停留在盲区中，并且持续一定时间的速度变化很小，则标记完成
  if (fabs(err_now) <= blind_area && fabs(DD) <= D_tolerance) {
    if (t_run.time() >= settle_time) settled = true;
  } else {
    t_run.reset();
    settled = false;
  }
  return puts;
}

float pid_ctrl::pid_output(float value_now) {
  val_now = value_now;
  err_now = aim - val_now;
  return pid_calc();
}

float pid_ctrl::pid_output_need_value(float aim_now, float value_now) {
  aim = aim_now;
  val_now = value_now;
  err_now = aim - val_now;
  return pid_calc();
}

float pid_ctrl::pid_output_need_err(float err_src) {
  err_now = err_src;
  return pid_calc();
}

void pid_ctrl::print() {
  if (print_detail) {
    printf("e_now:%.3f    puts:%.3f\n", err_now, puts);
    printf("                  II:%.3f      DD:%.3f\n", II, DD);
    printf("P_put:%.3f   I_put:%.3f   D_put:%.3f\n\n", P_put, I_put, D_put);
  }
}